/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/***********************************************************************
 *
 * FILENAME:
 * quatTest.cxx
 *
 * PURPOSE:
 * To test the quaternion class 
 *
 * LAST MODIFIED:
 * 11/18/2000  - Cory
 *
 ***********************************************************************/

#include <stdio.h>
#include <string.h>
#include <iostream.h>

#include "quaternion.h"

int main()
{
	Quaternion Qq; Qq(3) = 1;
	Quaternion Qr; Qr.set( PI/2.0, 1, 0, 0);
	Quaternion Qp(0, 0,  PI/2.0);
	
	cout << "Qq"<<  Qq << endl;
	cout << "Qr"<<  Qr << endl;
	cout << "Qp"<<  Qp << endl;

	Quaternion qq = ~Qp;

	cout << "~Qp"<<  qq << endl;
	cout << "(Qq == Qr)  = " << (Qq == Qr) << endl;
	cout << "(Qq != Qr)  = " << (Qq != Qr) << endl;

	Qp = Qq;
	cout << "Qp = Qq"<< Qp << endl;
	cout << "(Qp == Qq)  = " << (Qp == Qq) << endl;

	Qr= Qr*Qr*Qr*Qr;
	cout << "Half rotation q = q*q*q*q = " << Qr << endl;
	
	Qr.set(  PI/2.0, 0, 0, 2);
	cout << "norm(unit) = " << Qr.norm() << endl;
	cout << "unit = " << Qr.unit() << endl;
	cout << "norm(unit) = " << Qr.norm() << endl;

	Quaternion q;
	float qVec[4] = {0, 1, 0, 0};
	q = (Qr * Quaternion(qVec)) * (~Qr);
	cout << "Qr * [0 1 0 0] * ~Qr' = " << q << endl;
	cout << "~Qr * " << q <<" * Qr = "; q = ~Qr * q * Qr; cout << q << endl;

	cout << endl <<endl;

	q = ~Qr * Quaternion(qVec) * Qr;
	cout << "~Qr * [0 1 0 0]' * Qr = " << q << endl;
	cout << "Qr * " << q <<" * ~Qr = "; q = Qr * q * ~Qr; cout << q << endl;

	q.fromAngleAxis(  PI/2.0, 0, 1 , 0);
	cout << "fromAngleAxis( PI/2.0, 0, 1 , 0) = " << q << endl;

	float angle; 
	Matrix axis(3,1);
	q.toAngleAxis(angle, axis);
	cout << "toAngleAxis(angle, axis) => angle=" << angle 
		<<" axis = " << endl << axis <<endl;
	float x,y,h;
	q.toAngleAxis(angle, x, y, h);
	cout << "toAngleAxis(angle, axis) => angle=" << angle 
		<<" axis = [ " << x << " " << y << " " << h << " ] " << endl;

	float mat[] = { 1,0,0,0,0,1,0,-1,0 };
	Matrix m(3,3,mat);
	Quaternion a;

	q.fromMatrix(m);
	cout << "q.fromMatrix(m) " << q << endl;
	float Y_VEC[4] = {0, 1, 0, 0};
	a = q * Quaternion(Y_VEC) * ~q;
	cout << "q * [0 1 0 0]' * ~q = " << a << endl;
	Matrix z;
	q.toMatrix(z);
	float zz[] = {0, 1, 0};
	Matrix zzz(3,1,zz);
	cout << "q.toMatrix(z) => p: " << endl << z << endl;
	cout << " z * [ 0 1 0 ]' = " << endl << z * zzz << endl;

	q.fromEuler(00.0,  PI/2.0, 00.0);
	a = q * q.vec(1, 2, 3) * ~q;
	cout << "q * [1 2 3 0]' * ~q = " << a << endl;
	a = ~q *  q.vec(1, 2, 3) * q;
	cout << "~q * [1 2 3 0]' * q = " << a << endl;

	q.toEuler(z);
	cout << "Euler (0.0,  PI/2.0, 0.0): \n" << z << endl;

	q.fromEuler( PI/2.0,  PI/2.0,  PI/2.0);
	q.toEuler(z);
	cout << "Euler (90.0, 90.0, 90.0): \n" << z << endl;

	q.fromEuler(PI, PI, PI);
	float yaw, pitch, roll;
	q.toEuler(yaw,pitch,roll);
	cout << "Euler (180.0, 180.0, 180.0): \n" 
		<< "yaw= " << yaw 
		<< "pitch= " << pitch 
		<< "roll= " << roll << endl;
	q.toEuler(yaw,pitch,roll);
	Quaternion s(0,0,0,1);
	s.toEuler(yaw,pitch,roll);
	cout << "Euler (0,0,0,1): "
		<< "yaw= " << yaw 
		<< "pitch= " << pitch 
		<< "roll= " << roll << endl;

	s.fromEuler(0, 0, PI/2);
	cout << "Euler (0, 0, PI/2): " << s << endl;
	q.fromEuler(PI/2, 0, 0);
	s = q * s;	
	cout << "Roatation - " << s << endl;
	Quaternion b = ~s * Quaternion(1, 0,0,0) * s;
	cout << "Vector [1 0 0]b = " << b << endl;


	// create array of truth quaternions 
	// stepped every 0.1 sec for a rotation about yaw at a rate of pi/s
	Quaternion truth[10] = { Quaternion(0,0,0,1), Quaternion(0,0,0,1), 
				Quaternion(0,0,0,1), Quaternion(0,0,0,1),
				Quaternion(0,0,0,1), Quaternion(0,0,0,1),
				Quaternion(0,0,0,1), Quaternion(0,0,0,1),
				Quaternion(0,0,0,1), Quaternion(0,0,0,1)};
	Quaternion v(0,0,0,1);	
	Quaternion w(0,0,PI,0);	

	cout << endl << "Propagation of [0 0 0 1] by w=[0 0 pi] for T=1,2 " << endl;

	Quaternion p;

	p = v.propagate(w, 1000);
	cout << p << endl;
	v = v.propagate(w, 2000);
	cout << v << endl;

	cout << endl << "Propagation of [0 0 0 1] by w=[0 0 pi] for T=0...1 " << endl;
	for(float i=0; i < 2000; i+=200) {
		p = v.propagate(w, i);
		cout << "T=" << i << ": " <<p << endl;
	}
	cout << endl;


	float step = 200.0;
	cout << "T=" << 0 << ":\t" << v << endl;
	for(float i=1; i < 10; i++) {
		v = v.propagate(w, step);
		cout << "T=" << i*step << ": " << v << endl;
	}

	return 0;
}
